from motors.feetech import FeetechMotorsBus
from motors.configs import FeetechMotorsBusConfig
from motors.feetech import TorqueMode
from vr_monitor import VRMonitor
import time
import threading
import asyncio
import numpy as np
import math
import threading
# 配置串口参数和电机信息
config1 = FeetechMotorsBusConfig(
    port="/dev/ttyACM0",  # 串口端口
    motors={
            # name: (index, model)
            "shoulder_pan": [1, "sts3215"],
            "shoulder_lift": [2, "sts3215"],
            "elbow_flex": [3, "sts3215"],
            "wrist_flex": [4, "sts3215"],
            "wrist_roll": [5, "sts3215"],
            "gripper": [6, "sts3215"],
            }
)

config2 = FeetechMotorsBusConfig(
    port="/dev/ttyACM1",  # 串口端口
    motors={
            # name: (index, model)
            "shoulder_pan": [1, "sts3215"],
            "shoulder_lift": [2, "sts3215"],
            "elbow_flex": [3, "sts3215"],
            "wrist_flex": [4, "sts3215"],
            "wrist_roll": [5, "sts3215"],
            "gripper": [6, "sts3215"],
            }
)
# 初始化电机总线并建立串口连接
motors_bus_left = FeetechMotorsBus(config1)
motors_bus_right = FeetechMotorsBus(config2)
motors_bus_left.connect()  # 打开串口，建立通信
motors_bus_right.connect()  # 打开串口，建0立通信
# 读取时使用锁
# 改为顺序读取
all_positions_left = motors_bus_left.read("Present_Position")
print(all_positions_left)

all_positions_right = motors_bus_right.read("Present_Position")

print(all_positions_right)

# motors_bus_left.write("Torque_Enable", TorqueMode.ENABLED.value)
# motors_bus_right.write("Torque_Enable", TorqueMode.ENABLED.value)
# goal_pos = [2048, 2048, 2048, 2048, 2048, 2048]  # 对应shoulder、elbow、wrist、gripper
# motors_bus_left.write("Goal_Position", goal_pos)
# motors_bus_right.write("Goal_Position", goal_pos)


# def get_mapped_joints(left,right):
#     all_positions_left = left.read("Present_Position")
#     all_positions_right = right.read("Present_Position")
#     mapped_joints = np.zeros(16)
#     mapped_joints[0] = 0  # Base X position
#     mapped_joints[1] = 0  # Base rotation
    
#     # First arm: [3,6,9,11,13] → [2,3,4,5,6]
#     mapped_joints[2] = all_positions_left[0]
#     mapped_joints[3] = all_positions_left[1]
#     mapped_joints[4] = all_positions_left[2]
#     mapped_joints[5] = all_positions_left[3]
#     mapped_joints[6] = all_positions_left[4]
    
#     # Second arm: [4,7,10,12,14] → [7,8,9,10,11]
#     mapped_joints[7] = all_positions_right[0]
#     mapped_joints[8] = all_positions_right[1]
#     mapped_joints[9] = all_positions_right[2]
#     mapped_joints[10] = all_positions_right[3]
#     mapped_joints[11] = all_positions_right[4]
#     mapped_joints[12] = all_positions_left[5]
#     mapped_joints[13] = all_positions_right[5]
    
#     return mapped_joints

# def inverse_kinematics(x, y, l1=0.1159, l2=0.1350):

#     # Calculate joint2 and joint3 offsets in theta1 and theta2
#     theta1_offset = -math.atan2(0.028, 0.11257)  # theta1 offset when joint2=0
#     theta2_offset = -math.atan2(0.0052, 0.1349) + theta1_offset  # theta2 offset when joint3=0
    
#     # Calculate distance from origin to target point
#     r = math.sqrt(x**2 + y**2)
#     r_max = l1 + l2  # Maximum reachable distance
    
#     # If target point is beyond maximum workspace, scale it to the boundary
#     if r > r_max:
#         scale_factor = r_max / r
#         x *= scale_factor
#         y *= scale_factor
#         r = r_max
    
#     # If target point is less than minimum workspace (|l1-l2|), scale it
#     r_min = abs(l1 - l2)
#     if r < r_min and r > 0:
#         scale_factor = r_min / r
#         x *= scale_factor
#         y *= scale_factor
#         r = r_min
    
#     # Use law of cosines to calculate theta2
#     cos_theta2 = -(r**2 - l1**2 - l2**2) / (2 * l1 * l2)
    
#     # Calculate theta2 (elbow angle)
#     theta2 = math.pi - math.acos(cos_theta2)
    
#     # Calculate theta1 (shoulder angle)
#     beta = math.atan2(y, x)
#     gamma = math.atan2(l2 * math.sin(theta2), l1 + l2 * math.cos(theta2))
#     theta1 = beta + gamma
    
#     # Convert theta1 and theta2 to joint2 and joint3 angles
#     joint2 = theta1 - theta1_offset
#     joint3 = theta2 - theta2_offset
    
#     # Ensure angles are within URDF limits
#     joint2 = max(-0.1, min(3.45, joint2))
#     joint3 = max(-0.2, min(math.pi, joint3))
    
#     return joint2, joint3










